#include "collision_libccd.h"
#include "btBulletCollisionCommon.h"

void ccdGeomToObj(const btCollisionObject* collisionObject, ccd_obj_t *o)
{
    btVector3 btPos;
    btQuaternion btRot;

    btPos = collisionObject->getWorldTransform().getOrigin();
    btRot = collisionObject->getWorldTransform().getRotation();

    ccdVec3Set(&o->pos, btPos.getX(), btPos.getY(), btPos.getZ());
    ccdQuatSet(&o->rot, btRot.getX(), btRot.getY(), btRot.getZ(),btRot.getW());

    ccdQuatInvert2(&o->rot_inv, &o->rot);
}

void ccdGeomToCyl(const btCollisionObject* collisionObject, ccd_cyl_t *cyl)
{
    //btScalar r, h;
    ccdGeomToObj(collisionObject, (ccd_obj_t *)cyl);

	btCylinderShape* cylinderShape = (btCylinderShape*)collisionObject->getCollisionShape();
    cyl->radius = cylinderShape->getRadius();
    cyl->height = cylinderShape->getHalfExtentsWithMargin().getZ();
}

void ccdGeomToBox(const btCollisionObject* collisionObject, ccd_box_t *box)
{
    //btVector3 dim;
	ccdGeomToObj(collisionObject, (ccd_obj_t *)box);

	btBoxShape* boxShape = (btBoxShape*)collisionObject->getCollisionShape();
	box->dim[0] = boxShape->getHalfExtentsWithoutMargin().getX();
    box->dim[1] = boxShape->getHalfExtentsWithoutMargin().getY();
    box->dim[2] = boxShape->getHalfExtentsWithoutMargin().getZ();
}

int ccdCollide(const btCollisionObject* o1, const btCollisionObject* o2, int flags,
                      ContactGeom *contact, int skip,
                      void *obj1, ccd_support_fn supp1, ccd_center_fn cen1,
                      void *obj2, ccd_support_fn supp2, ccd_center_fn cen2)
{
    ccd_t ccd;
    int res;
    ccd_real_t depth;
    ccd_vec3_t dir, pos;
    int max_contacts = (flags & 0xffff);

    if (max_contacts < 1)
        return 0;

    CCD_INIT(&ccd);
    ccd.support1 = supp1;
    ccd.support2 = supp2;
    ccd.center1  = cen1;
    ccd.center2  = cen2;
    //ccd.max_iterations = 500;
    //ccd.mpr_tolerance = 1E-6;
    ccd.max_iterations = 1;
    ccd.mpr_tolerance = 0.134;//0.067;


   /* if (flags & CONTACTS_UNIMPORTANT){
        if (ccdMPRIntersect(obj1, obj2, &ccd)){
            return 1;
        }else{
            return 0;
        }
    }*/

    res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
    if (res == 0){
        contact->g1 = o1;
        contact->g2 = o2;

        contact->side1 = contact->side2 = -1;

        contact->depth = depth;

        contact->pos[0] = ccdVec3X(&pos);
        contact->pos[1] = ccdVec3Y(&pos);
        contact->pos[2] = ccdVec3Z(&pos);

        ccdVec3Scale(&dir, -1.);
        contact->normal[0] = ccdVec3X(&dir);
        contact->normal[1] = ccdVec3Y(&dir);
        contact->normal[2] = ccdVec3Z(&dir);

        return 1;
    }

    return 0;
}

void ccdSupportCyl(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v)
{ 
    const ccd_cyl_t *cyl = (const ccd_cyl_t *)obj;
    ccd_vec3_t dir;
    double zdist, rad;

    ccdVec3Copy(&dir, _dir);
    ccdQuatRotVec(&dir, &cyl->o.rot_inv);

    zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
    zdist = sqrt(zdist);
    if (ccdIsZero(zdist)){
        ccdVec3Set(v, 0., 0., ccdSign(ccdVec3Z(&dir)) * cyl->height);
    }else{
        rad = cyl->radius / zdist;

        ccdVec3Set(v, rad * ccdVec3X(&dir),
                      rad * ccdVec3Y(&dir),
                      ccdSign(ccdVec3Z(&dir)) * cyl->height);
    }

    // transform support vertex
    ccdQuatRotVec(v, &cyl->o.rot);
    ccdVec3Add(v, &cyl->o.pos);
}

void ccdSupportBox(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v)
{
    const ccd_box_t *o = (const ccd_box_t *)obj;
    ccd_vec3_t dir;

    ccdVec3Copy(&dir, _dir);
    ccdQuatRotVec(&dir, &o->o.rot_inv);

    ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0],
                  ccdSign(ccdVec3Y(&dir)) * o->dim[1],
                  ccdSign(ccdVec3Z(&dir)) * o->dim[2]);

    // transform support vertex
    ccdQuatRotVec(v, &o->o.rot);
    ccdVec3Add(v, &o->o.pos);
}

void ccdCenter(const void *obj, ccd_vec3_t *c)
{
    const ccd_obj_t *o = (const ccd_obj_t *)obj;
    ccdVec3Copy(c, &o->pos);
}

